#include <PID_v1.h>
#include <Wire.h>   //For I2C
#include <avr/wdt.h>  //For Watchdog
#include <PWM.h>  //For PWM

#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif 

 #define I2C_ADDRESS 2 //CHANGED FOR EACH MOTOR CONTROLLER
 #define I2C_BYTES_FROM_MASTER 12

#define PWM_FREQ 100 // in Hz

#define PID_SAMPLING_TIME 100 //in ms

#define TICK_LOWER_THRESH 20 //smallest delta between previous tick time and current tick time
#define TICK_UPPER_THRESH 200

#define MIN_SPEED_DELTA 200 //how many milliseconds before ticks before the motor is considered stopped

#define MOTOR1 1
#define MOTOR2 2
#define MOTOR3 3

//Motor 1//
#define M1_PWM_PIN 5 // [http://playground.arduino.cc/Main/TimerPWMCheatsheet]
#define M1_SPEED_PIN 19 //interrupt pin [http://arduino.cc/en/Reference/attachInterrupt]
#define M1_DIRECTION_PIN 22
#define M1_CURRENT_ANALOG_PIN 9
#define M1_TEMPERATURE_ANALOG_PIN 1


//Motor 2//
#define M2_PWM_PIN 12 
#define M2_SPEED_PIN 3 
#define M2_DIRECTION_PIN 50
#define M2_CURRENT_ANALOG_PIN 2
#define M2_TEMPERATURE_ANALOG_PIN 3

//Motor 3//
#define M3_PWM_PIN 9
#define M3_SPEED_PIN 19
#define M3_DIRECTION_PIN 46
#define M3_CURRENT_ANALOG_PIN 4
#define M3_TEMPERATURE_ANALOG_PIN 5

#define SPEED_ARRAY_SIZE 20
#define TEMPERATURE_ARRAY_SIZE 100
#define CURRENT_ARRAY_SIZE 10
#define CURRENT_READS 100
#define FORWARD 1
#define BACKWARD 0

#define DIRECTION_TOGGLE_COOLDOWN 200 //when toggling direction, how much time to wait for the motors to come to a steop, in milliseconds

 
class Motor;

//Misc
volatile int directionDesired = FORWARD; //keep the previous state of the direction
volatile int directionCurrent = FORWARD;
//volatile bool toggleDirection = false; //if the direction needs to be tweaked 
volatile int motorIndexSelect = 0;
volatile int compb_i = 0;
volatile int adc_reading = 0;
volatile long currentWaitTime = (long)( ( ((float)1/PWM_FREQ) * 3) * 1000 ); //we want to wait the period of a PWM signal, times 3 (engineering safety factor), times 1000 to get milliseconds //when measuring current, we wait a set amount of time to guarantee the PWM signals goes up and then drops at least once
volatile bool gotI2cCommand = false;

//I2C stuff
uint8_t i2cReceiveBuffer[12]; //will contain, for example, F255F255F255\

volatile uint8_t motor1_command[4];
volatile uint8_t motor2_command[4];
volatile uint8_t motor3_command[4];

uint8_t i2cSendBuffer[15];
uint8_t i2cSendBufferTesting[9];
uint8_t i2cSendBufferTemporary[15];
size_t sendBufferSize = 15;



Motor* motors[3]; //holding the pointers to the 3 motors so the 3 motors can be called from static methods

class Motor
{

 public:
  
  
  int motorNumber;
  
  long tick_delta;
  int speedDeltaIndex;
  long previous_tick;
  bool newTick;
  long currentIndex;
  
  
  static int adc_reading;

  int speedDelta[SPEED_ARRAY_SIZE];
  int temperature [TEMPERATURE_ARRAY_SIZE];  //in Celsius
  int current[CURRENT_ARRAY_SIZE];  //in amperes
  float actualCurrent;      //stores the moving average calculated from current[]
  float actualTemperature;  //stores the moving average calculated from temperature[]
  int pwmPin; 
  int currentPin;

  double speedSensorCoefficient;

  float zero_current;

  //PID
  PID* pid;
  double RPM;
  double duty;
  double desiredRPM;
  double kp;
  double ki;
  double kd;

  int speedPin;
  int directionPin;
  int temperaturePin;

  Motor(int pwm_pin_, int directionPin_, int temperaturePin_, int speed_pin_, int current_analog_pin_, int motorNum_)
  {
	  speedPin = speed_pin_;
	  directionPin = directionPin_;
	  temperaturePin = temperaturePin_;
	  motorNumber = motorNum_;
	  speedDeltaIndex = 0;
	  pwmPin = pwm_pin_;
	  currentPin = current_analog_pin_;
	  speedSensorCoefficient = 1;
	  currentIndex = 0;

	  /*zero_current = 0;
	  for(int i = 0; i < CURRENT_ARRAY_SIZE; i++)
	  {
		  zero_current += analogRead(currentPin);
	  }
	  zero_current = zero_current / CURRENT_ARRAY_SIZE;*/

	  RPM = 0;
	  duty = 0;
	  desiredRPM = 0;
	  kp = 1;
	  ki = 1;
	  kd = 1;

	  SetPinFrequencySafe(pwmPin, PWM_FREQ);
	  pwmWrite(pwmPin, 255);

	  pinMode(this->directionPin, OUTPUT);
	  digitalWrite(this->directionPin, directionCurrent);

	  /*pid = new PID(&RPM, &duty, &desiredRPM, kp, ki, kd, DIRECT);
	  pid->SetMode(AUTOMATIC);
      pid->SetSampleTime(PID_SAMPLING_TIME);
	  pid->SetOutputLimits(0, 255);*/
	  
	  newTick = false;
	  previous_tick = 0;

	  for(int i = 0 ; i < SPEED_ARRAY_SIZE ; i++)
	  {
		speedDelta[i] = 0;
	  }


	  for(int i = 0 ; i < TEMPERATURE_ARRAY_SIZE; i++)
	  {
		temperature[i] = 0;
	  }

	  for(int i = 0 ; i < CURRENT_ARRAY_SIZE; i++)
	  {
		current[i] = 0;
	  }

	/*  int actual_interrupt_number = PinToInterruptNumber(speedPin);
	 
	  switch(motorNumber)
	  {
		  case MOTOR1:
			  attachInterrupt(actual_interrupt_number,motor1SpeedTickInterrupt,FALLING);
			  break;
		  case MOTOR2:
			  attachInterrupt(actual_interrupt_number,motor2SpeedTickInterrupt,FALLING);
			  break;
		  case MOTOR3:
			  attachInterrupt(actual_interrupt_number,motor3SpeedTickInterrupt,FALLING);
			  break;
		  default:
			  Serial.print("Motor number is unknown:");Serial.println(motorNumber);
	  }*/



	  
	  
}

  void updateTemperature()
  {
	  long average = 0;

	  for(int i = 0; i < TEMPERATURE_ARRAY_SIZE; i++)
	  {
		  //average += analogRead(this->temperaturePin); 
		  average += analogRead(1); 
	  }

	  average /= TEMPERATURE_ARRAY_SIZE;

	  this->actualTemperature = 4.883 / 10 * (float)average;
  }

  void updateSpeed()
  {
	  //checking if the motor is stopped or very slow; a new tick has not happened in MIN_SPEED_DELTA milliseconds so the teeth are moving slowly or not at all
	  if( (millis() - previous_tick) > MIN_SPEED_DELTA ) //need to verify current the motor is pulling to determine stop or going slowly
	  {
		  RPM = 0;//10 * speedSensorCoefficient; //temporary
		  return;
	  }
	  else if(newTick == true)
	  {
		long average = 0;
		for(int i = 0; i < SPEED_ARRAY_SIZE; i++)
		{
			//Serial.println(speedDelta[i]);
			average += speedDelta[i];
		}

		average /= SPEED_ARRAY_SIZE;
		this->RPM = (double)1500 / average; //3000 or 1500
		//this->RPM = this->RPM * speedSensorCoefficient; //calibration 
	  }
  }

  void updateCurrent()
  {
	  float average = 0;



	  for(int i = 0; i < CURRENT_READS; i++)
	  {
		  average += analogRead(this->currentPin);  
	  }
	  
	  average /= CURRENT_READS;
	  average = zero_current - average  ; //adjusting for the calibration current reading


	  current[currentIndex%CURRENT_ARRAY_SIZE] = 4.883 / 27.9 * (float)average; //
	  currentIndex++;

	  float average2 = 0;
	  for(int j = 0 ; j  < CURRENT_ARRAY_SIZE; j++)
	  {
		  average2 += current[j];
	  }

	  average2 /= CURRENT_ARRAY_SIZE;

	  this->actualCurrent = average2;
	  //this->actualCurrent = 4.883 / 27.9 * (float)average; //
  }

  //void setDutyCycle()
  //{
	 // setDutyCycle((int)this->duty);
  //}

  void setDutyCycle(int duty)
  {
	  pwmWrite(this->pwmPin, 255 - (duty*0.35));
  }

  void setDirection(int dir)
  {
	  switch(dir)
	  {
	  case FORWARD:
		  digitalWrite(this->directionPin, FORWARD);
		  break;
	  case BACKWARD:
		  digitalWrite(this->directionPin, BACKWARD);
		  break;
	  default:
		  break;

	  }

  }

  static void motor1SpeedTickInterrupt()
  {
		Motor* m1 = motors[0];
	    long current_tick = millis();
		m1->tick_delta = current_tick  - (m1->previous_tick) ;
		if(m1->tick_delta > TICK_LOWER_THRESH && m1->tick_delta < TICK_UPPER_THRESH)
		{
			m1->newTick = true;
			m1->speedDelta[m1->speedDeltaIndex%SPEED_ARRAY_SIZE] = m1->tick_delta;
			m1->previous_tick = current_tick;
			(m1->speedDeltaIndex)++;
		}
		m1->previous_tick = current_tick;

  }
  
  static void motor2SpeedTickInterrupt()
  {
		Motor* m1 = motors[1];
	    long current_tick = millis();
		m1->tick_delta = current_tick  -(m1->previous_tick) ;
		if(m1->tick_delta > TICK_LOWER_THRESH && m1->tick_delta < TICK_UPPER_THRESH)
		{
			m1->newTick = true;
			m1->speedDelta[m1->speedDeltaIndex%SPEED_ARRAY_SIZE] = m1->tick_delta;
			m1->previous_tick = current_tick;
			(m1->speedDeltaIndex)++;
		}
		m1->previous_tick = current_tick;
  
  }
  
  static void motor3SpeedTickInterrupt()
  {
  
  
  }

  static int PinToInterruptNumber(int pin)
  {
	   switch(pin) //pin as labeled on the Arduino isn't exactly the interrupt number http://arduino.cc/en/Reference/attachInterrupt
	  {
		  case 2:
			  return 0;
			  break;
		  case 3:
			  return 1;
			  break;
		  case 21:
			  return 2;
			  break;
		  case 20:
			  return 3;
			  break;
		  case 19:
			  return 4;
			  break;
		  case 18:
			  return 5;
			  break;
		  default:
			Serial.print("Could not find the interrupt number associated with analog pin ");Serial.println(pin);
			return -1;
	  }

  }
  
  //prepare the ADC for a particular motor
 // static void setCurrentAdc(int motorNumber)
 // {

	////ADC configuration for less resolution but faster sampling
	////ADPS2	ADPS1	ADPS0	Division Factor
	////0	0	0	2
	////0	0	1	2
	////0	1	0	4
	////0	1	1	8
	////1	0	0	16
	////1	0	1	32
	////1	1	0	64
	////1	1	1	128
	//sbi(ADCSRA, ADPS2); 
	//cbi(ADCSRA, ADPS1);
	//cbi(ADCSRA, ADPS0);

 //   ADCSRA |= (1<<ADEN) |(1<<ADIE); //enable ADC, enable ADC interrupt vector
 //   ADMUX |= (1 << ADLAR); //left aligning

	////interrupts when conversion is complete
	//sbi(ADCSRA, ADIF);

	////set AREF source (VCC with cap across it)
	//sbi(ADMUX, REFS0);

	////sets which analog pin will be read
	////sets what PWM interrupts will be used
	//switch(motorNumber)
	//{
	//case MOTOR1:
	//	
	//	

	//	break;
	//case MOTOR2:
	//	

	//	break;
	//case MOTOR3:
	//	

	//	break;
	//default:
	//	Serial.println("In setAdc(), motorNumber was not a predetermined value");
	//	break;
	//}


 // }

};

void setup()
{
	 
    Serial.begin(9600); 
	Serial.println("System started");
	InitTimersSafe(); //initialize all timers except for 0, to save time keeping functions

	 Motor* m1 = new Motor(M1_PWM_PIN, M1_DIRECTION_PIN, M1_TEMPERATURE_ANALOG_PIN, M1_SPEED_PIN, M1_CURRENT_ANALOG_PIN, MOTOR1);
	 Motor* m2 = new Motor(M2_PWM_PIN, M2_DIRECTION_PIN, M2_TEMPERATURE_ANALOG_PIN, M2_SPEED_PIN, M2_CURRENT_ANALOG_PIN, MOTOR2);
	 Motor* m3 = new Motor(M3_PWM_PIN, M3_DIRECTION_PIN, M3_TEMPERATURE_ANALOG_PIN, M3_SPEED_PIN, M3_CURRENT_ANALOG_PIN, MOTOR3);
	 motors[0] = m1;
	 motors[1] = m2;
	 motors[2] = m3;

     initI2C(I2C_ADDRESS);
   
	 attachInterrupt(4,Motor::motor1SpeedTickInterrupt,FALLING);

	 motors[0]->zero_current = 0;
	 
	  for(int i = 0; i < CURRENT_READS; i++)
	  {
		  motors[0]->zero_current += analogRead(motors[0]->currentPin);

	  }
	  motors[0]->zero_current = motors[0]->zero_current / CURRENT_READS;

	  pinMode(A0, INPUT	);

 //Setup Watchdog
	 
   wdt_enable(WDTO_1S); //reset uC after 1 second
 
}


 
void loop()
{	
	asm volatile("wdr");
	delay(100);

	ParseI2cInput();
	setDirection();


	
	Serial.print("Desired duty:");Serial.print(motors[0]->desiredRPM);
	Serial.print("  RPM:"); Serial.print(motors[0]->RPM);
	Serial.print("  Current:"); Serial.print(motors[0]->actualCurrent);
	Serial.print("  Temp:");  Serial.print( (motors[0]->actualTemperature) );
	Serial.println("");

	//Serial.print("                    ");
	
	//Serial.write(27);       // ESC command
 // Serial.print("[2J");    // clear screen command
 // Serial.write(27);
 // Serial.print("[H");     // cursor to home command
	////Serial.print("[H");     // cursor to home command
	//Serial.print("\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b");

	motors[0]->updateTemperature();
	//motors[1]->updateTemperature();

	motors[0]->updateCurrent();
	//motors[1]->updateCurrent();
	
	//motors[1]->updateSpeed();
	motors[0]->updateSpeed();
	
    pwmWrite(M1_PWM_PIN, 255 - (int)(0.35*(motors[0]->desiredRPM)));

	//motors[0]->setDutyCycle(motors[0]->desiredRPM);
	//motors[1]->setDutyCycle(motors[1]->desiredRPM);
}

void initI2C(int address)
{
 Wire.begin(address);                // join i2c bus 
 Wire.onRequest(I2cRequest); // register event
 Wire.onReceive(I2cReceive); // register event
}


void I2cRequestTest()
{
	i2cSendBufferTesting[0] = (uint8_t)motors[0]->RPM;
	i2cSendBufferTesting[1] = (uint8_t)motors[0]->desiredRPM;
	i2cSendBufferTesting[2] = (uint8_t)motors[0]->duty;

	i2cSendBufferTesting[3] = (uint8_t)motors[1]->RPM;
	i2cSendBufferTesting[4] = (uint8_t)motors[1]->desiredRPM;
	i2cSendBufferTesting[5] = (uint8_t)motors[1]->duty;

	i2cSendBufferTesting[6] = (uint8_t)motors[2]->RPM;
	i2cSendBufferTesting[7] = (uint8_t)motors[2]->desiredRPM;
	i2cSendBufferTesting[8] = (uint8_t)motors[2]->duty;

	Wire.write(i2cSendBufferTesting, (size_t)9);

}


void I2cRequest()
{
	//motor 1 speed
	i2cSendBuffer[0] = (uint8_t)motors[0]->RPM;
	//motor 1 current
	i2cSendBuffer[1] = (uint8_t)motors[0]->actualCurrent;
	//motor 1 temperature 1
	i2cSendBuffer[2] = (uint8_t)motors[0]->actualTemperature;
	//motor 1 temperature 2
	i2cSendBuffer[3] = (uint8_t)motors[0]->actualTemperature;
	//motor 1 temperature 3
	i2cSendBuffer[4] = (uint8_t)motors[0]->actualTemperature;
  
	//same for motors 2 and 3
	//motor 2 speed
	i2cSendBuffer[5] = (uint8_t)motors[1]->RPM;
	//motor 2 current
	i2cSendBuffer[6] = (uint8_t)motors[1]->actualCurrent;
	//motor 2 temperature 1
	i2cSendBuffer[7] = (uint8_t)motors[1]->actualTemperature;
	//motor 2 temperature 2
	i2cSendBuffer[8] = (uint8_t)motors[1]->actualTemperature;
	//motor 2 temperature 3
	i2cSendBuffer[9] = (uint8_t)motors[1]->actualTemperature;

	//motor 3 speed
	i2cSendBuffer[10] = (uint8_t)motors[2]->RPM;
	//motor 3 current
	i2cSendBuffer[11] = (uint8_t)motors[2]->actualCurrent;
	//motor 3 temperature 1
	i2cSendBuffer[12] = (uint8_t)motors[2]->actualTemperature;
	//motor 3 temperature 2
	i2cSendBuffer[13] = (uint8_t)motors[2]->actualTemperature;
	//motor 3 temperature 3
	i2cSendBuffer[14] = (uint8_t)motors[2]->actualTemperature;

	Wire.write(i2cSendBuffer, sendBufferSize);


}

void I2cReceive(int bytesFromMaster)
{
	//Serial.println("SOMETHING");
	if(bytesFromMaster == 12)
	{
		
		for(int i = 0 ; i < 12; i++)
		{
			i2cReceiveBuffer[i] = Wire.read();
		}
		gotI2cCommand = true;		
	}
	else if(bytesFromMaster < 12)
	{
		Serial.println("I2C master wrote LESS than 12 bytes");
		for(int i = 0 ; i < 12; i++)
		{
			i2cReceiveBuffer[i] = Wire.read();
		}
		gotI2cCommand = true;
	}
	else
	{
		Serial.println("I2C master wrote MORE than 12 bytes");
	}

}

void ParseI2cInput()
{
	
	if(gotI2cCommand == true)
	{
		gotI2cCommand = false;
		motor1_command[0] = i2cReceiveBuffer[1];
		motor1_command[1] = i2cReceiveBuffer[2];
		motor1_command[2] = i2cReceiveBuffer[3];
		motor1_command[3] = '\0';

		double m1value =  (double)(atof((char*)motor1_command)) ;


		motor2_command[0] = i2cReceiveBuffer[5];
		motor2_command[1] = i2cReceiveBuffer[6];
		motor2_command[2] = i2cReceiveBuffer[7];
		motor2_command[3] = '\0';

		double m2value =  (double)(atof((char*)motor2_command)) ;

		
		motor3_command[0] = i2cReceiveBuffer[9];
		motor3_command[1] = i2cReceiveBuffer[10];
		motor3_command[2] = i2cReceiveBuffer[11];
		motor3_command[3] = '\0';

		double m3value =  (double)(atof((char*)motor3_command)) ;

		switch(i2cReceiveBuffer[0])
		{
		case 'F':
			
			directionDesired = FORWARD;
			motors[0]->desiredRPM = m1value;
			motors[1]->desiredRPM = m2value;
			motors[2]->desiredRPM = m3value;

			break;
		case 'B':
			directionDesired = BACKWARD;
			motors[0]->desiredRPM = m1value;
			motors[1]->desiredRPM = m2value;
			motors[2]->desiredRPM = m3value;
			break;
		case 'P':
			motors[0]->kp = m1value;
			motors[1]->kp = m2value;
			motors[2]->kp = m3value;


			break;
		case 'I':
			motors[0]->ki = m1value;
			motors[1]->ki = m2value;
			motors[2]->ki = m3value;

			break;
		case 'D':
			motors[0]->kd = m1value;
			motors[1]->kd = m2value;
			motors[2]->kd = m3value;

			break;
		case 'E':
			motors[0]->speedSensorCoefficient = m1value/10;
			motors[1]->speedSensorCoefficient = m2value/10;
			motors[2]->speedSensorCoefficient = m3value/10;
			break;
		default:
			break;
		}
	}


}


void processUserInput(int pwmPin_)
{

	Serial.print("Writing on pwm pin "); Serial.println(pwmPin_);
		  int duty;
			
  // read the incoming byte:
          int incomingByte = Serial.read();
          
          switch(incomingByte)
          {
            /*case 'q':
            duty = 0;
            pwmWrite(pwmPin_, duty);
            delay(100);
            Serial.println("Switching directions");
            if(forward == true)
            {
               digitalWrite(directionPin, LOW);
               forward = false; 
            }
            else
            {
              digitalWrite(directionPin, HIGH);
              forward = true;
            }
            
            delay(100);*/
            
            
            
           case 'w':
             Serial.print("More!");
             if( duty != 240)
             {
 
               duty = duty + 30;  
             }
 

             break;  
            
            case 's':
             Serial.print("Less!");
             if(duty != 0 && (duty - 30) > 0)
             {
 
               duty = duty - 30;               
             }
 
            
 
             break; 
           
           case '1':
            duty = 0;
            break;
            
           case '2':
            duty = 30;
            break; 
            
            case '3':
            duty = 60;
            break; 
            
            case '4':
            duty = 90;
            break; 
            
            case '5':
            duty = 120;
            break; 
            
            case '6':
            duty = 150;
            break; 
            
            case '7':
            duty = 180;
            break; 
            
            case '8':
            duty = 210;
            break; 
            
            case '9':
            duty = 240;
            break; 
            
            case '0': //'p' keyboard letter
            duty = 255;
            break;
 
               
            default:
            Serial.print("w key for more, s key for less. Preset values at 1, 2, 3, 4, 5, 6, 7, 8, 9, 0 (1 is lowest, 0 is highest). ");
            Serial.println("q toggles direction");  
//            Serial.print(incomingByte, DEC);
          }
          Serial.print(" Now at:");
          Serial.println(duty, DEC);
          pwmWrite(pwmPin_, duty);
  



}


void updateWinFormData()
{
  //delay(100);
  char TempChar[10];  //  Hold The Convert Data
  
  String report = "R1";
  dtostrf(motors[0]->RPM,3,2,TempChar);
  report += String(TempChar); 
  report += "|";

  report += "R2";
  dtostrf(motors[1]->RPM,3,2,TempChar);
  report += String(TempChar); 
  report += "|";

  report += "R3";
  dtostrf(motors[2]->RPM,3,2,TempChar);
  report += String(TempChar); 
  report += "|";

  Serial.print(report);
  

  report = "";
  
  report += "D1";
  dtostrf(motors[0]->duty,3,2,TempChar);
  report += String(TempChar); 
  report += "|";

  report += "D2";
  dtostrf(motors[1]->duty,3,2,TempChar);
  report += String(TempChar); 
  report += "|";

  report += "D3";
  dtostrf(motors[2]->duty,3,2,TempChar);
  report += String(TempChar); 
  report += "|";
 
  Serial.print(report);
   report = "";
   
  report += "S1";
  dtostrf(motors[0]->desiredRPM,3,2,TempChar);
  report += String(TempChar); 
  report += "|";

  report += "S2";
  dtostrf(motors[1]->desiredRPM,3,2,TempChar);
  report += String(TempChar); 
  report += "|";

  report += "S3";
  dtostrf(motors[2]->desiredRPM,3,2,TempChar);
  report += String(TempChar); 
  report += "|";
   
  Serial.print(report);
   report = "";
   
  report += "p1";
  dtostrf(motors[0]->kp,3,2,TempChar);
  report += String(TempChar); 
  report += "|";

  report += "p2";
  dtostrf(motors[1]->kp,3,2,TempChar);
  report += String(TempChar);
  report += "|";
  
  report += "p3";
  dtostrf(motors[2]->kp,3,2,TempChar);
  report += String(TempChar);
  report += "|";

   Serial.print(report);
   report = "";
   
  report += "i1";
  dtostrf(motors[0]->ki,3,2,TempChar);
  report += String(TempChar); 
  report += "|";

  report += "i2";
  dtostrf(motors[1]->ki,3,2,TempChar);
  report += String(TempChar); 
  report += "|";

  report += "i3";
  dtostrf(motors[2]->ki,3,2,TempChar);
  report += String(TempChar); 
  report += "|";

   Serial.print(report);
   report = "";
   
  report += "d1";
  dtostrf(motors[0]->kd,3,2,TempChar);
  report += String(TempChar);
  report += "|";

  report += "d2";
  dtostrf(motors[1]->kd,3,2,TempChar);
  report += String(TempChar);
  report += "|";
 
  report += "d3";
  dtostrf(motors[2]->kd,3,2,TempChar);
  report += String(TempChar);
  report += "|";
 
   Serial.print(report);
   report = "";
   delay(100);
  report += "C1";
  dtostrf(motors[0]->actualCurrent,3,2,TempChar);
  report += String(TempChar);
  report += "|";

  report += "C2";
  dtostrf(motors[1]->actualCurrent,3,2,TempChar);
  report += String(TempChar);
  report += "|";

  report += "C3";
  dtostrf(motors[2]->actualCurrent,3,2,TempChar);
  report += String(TempChar);
  report += "|";
  
   Serial.print(report);
   report = "";
   

  report += "T1";
  dtostrf(motors[0]->actualTemperature,3,2,TempChar);
  report += String(TempChar);
  report += "|";
  
  report += "T2";
  dtostrf(motors[1]->actualTemperature,3,2,TempChar);
  report += String(TempChar);
  report += "|";
  
  report += "T3";
  dtostrf(motors[2]->actualTemperature,3,2,TempChar);
  report += String(TempChar);
  report += "|";
 
	Serial.print(report);
   report = "";
   
   if(directionDesired == FORWARD)
   {
	   report+= "t9111";
   }
   else
   {
	   report+= "t9000";
   }
  report += "|";
  Serial.print(report);
  
}

void setDirection()
{
	if(directionDesired != directionCurrent)
	{

		motors[0]->setDutyCycle(0);
		motors[1]->setDutyCycle(0);
		motors[2]->setDutyCycle(0);
		delay(DIRECTION_TOGGLE_COOLDOWN);
		
		motors[0]->setDirection(directionDesired);
		motors[1]->setDirection(directionDesired);
		motors[2]->setDirection(directionDesired);
		directionCurrent = directionDesired;

	}
}


